這個算法使用的是兩張的深度圖,將他們轉換成 3D 點雲,然後估計兩個點雲之間的 3D 轉換(一個旋轉矩陣和位移向量),這樣就可以得到兩個相機的位移和旋轉。
類似於 RGB 圖片的相機姿態估計,首先要能夠知道兩個點雲之間的匹配關係,並且用這個關係作為約束來估計相機的位移和旋轉。
如算法名稱所示,要得到這個匹配關係,直接在另一個點雲中找到最近點 (Nearest neighbor) ,簡單、暴力。
如果已知匹配,就可以透過解出最小平方誤差的方式來得到相機的位移和旋轉。假設有兩個點雲 和 (將所有的 3D 點合併成一個 的矩陣, 點雲是裡面的某一個點),並且兩個點雲按最近點匹的順序一一對應,我們要估計的是一個旋轉矩陣 和一個平移向量 ,使得 能夠和 最接近,也就是說我們要最小化下面的誤差函數:
同時估計未知的 較為困難,因此我們可以讓問題簡單一些,也就是先估計 ,再估計 。
要省略位移的方法就是把將兩個點雲移的質心移到原點,只估計一個旋轉來最小化點雲之間的誤差。 和 的質心分別是:
然後我們可以得到 和 的零均值點雲:
接著要使用 SVD 分解來估計 ,這裡省略推導,直接給出結果:
最後,我們可以估計 ,可以直接使用經過旋轉質心的差:
這樣就可以得到在這個點與點之間匹配下的相機位移和旋轉,不過這個匹配只是用最近點估計出來的,肯定不是完美的,因此我們可以重複這個過程多次,直到收斂,隨著兩個點雲的旋轉和位移越來越正確,那麼兩個點雲之間的匹配也會越來越好。